#include "stdafx.h"
#include "Quaternion.h"
#include "math/Math.h"

using namespace unigame;

void Quaternion::set(const Vector3f& v, float angle)
{
	float r = Math::angToRad(angle/ 2);
	w = cosf(r);
	float s = sinf(r);
	x = v.x * s;
	y = v.y * s;
	z = v.z * s;
}

void Quaternion::setEulerByRad(const float x, const float y, const float z)
{
	float rad = x * 0.5f;
	float sx = sinf(rad);
	float cx = cosf(rad);

	rad = y * 0.5f;
	float sy = sinf(rad);
	float cy = cosf(rad);

	rad = z * 0.5f;
	float sz = sinf(rad);
	float cz = cosf(rad);

	float sxcy = sx * cy;
	float sycx = sy * cx;
	float sxsy = sx * sy;
	float cxcy = cx * cy;
	
	w = cxcy * cz + sxsy * sz;
	this->x = sxcy * cz + sycx * sz;
	this->y = sycx * cz - sxcy * sz;
	this->z = cxcy * sz - sxsy * cz;
}

void Quaternion::fromTo(const Vector3f& from, const Vector3f& to)
{
	Vector3f v1 = from;
	Vector3f v2 = to;
	v1.normalize();
	v2.normalize();

	float dot = v1.dot(v2);
	if(Math::floatBiggerOrEqual(dot, 1.0f))
	{
		setToIdentity();
		return;
	}
	if(Math::floatSmallerOrEqual(dot, -1.0f))
	{
		Vector3f axie(1, 0, 0);
		axie = axie.cross(from);
		if(Math::floatEqual(axie.squrMag(), 0))
		{
			axie.set(0, 1, 0);
			axie = axie.cross(from);
		}
		axie.normalize();
		w = 0;
		x = axie.x;
		y = axie.y;
		z = axie.z;
		return;
	}
	float _2cosHalf = sqrtf((1 + dot) * 2);
	Vector3f a = v1.cross(v2);
	w = _2cosHalf * 0.5f;
	float s = 1 / _2cosHalf;
	a *= s;
	x = a.x;
	y = a.y;
	z = a.z;
}

void Quaternion::toMatrix(Matrixf& m)
{
	float temp[12];
	float y2 = 2 * y * y;
	float z2 = 2 * z * z;
	float x2 = 2 * x * x;
	float xy = 2 * x * y;
	float xz = 2 * x * z;
	float xw = 2 * x * w;
	float wy = 2 * w * y;
	float yz = 2 * y * z;
	float wz = 2 * w * z;

	temp[M_INDEX(0, 0)] = 1 - y2 - z2;
	temp[M_INDEX(0, 1)] = xy - wz;
	temp[M_INDEX(0, 2)] = xz + wy;
	temp[M_INDEX(0, 3)] = 0;

	temp[M_INDEX(1, 0)] = xy + wz;
	temp[M_INDEX(1, 1)] = 1 - x2 - z2;
	temp[M_INDEX(1, 2)] = yz - xw;
	temp[M_INDEX(1, 3)] = 0;

	temp[M_INDEX(2, 0)] = xz - wy;
	temp[M_INDEX(2, 1)] = yz + xw;
	temp[M_INDEX(2, 2)] = 1 - x2 - y2;
	temp[M_INDEX(2, 3)] = 0;

/*	temp[M_INDEX(3, 0)] = 0;
	temp[M_INDEX(3, 1)] = 0;
	temp[M_INDEX(3, 2)] = 0;
	temp[M_INDEX(3, 3)] = 1;*/

	m.setValue(temp);
}

Quaternion& Quaternion::operator =(const Matrixf& m)
{
	float wtemp = m(0, 0) + m(1, 1) + m(2, 2);
	float xtemp = m(0, 0) - m(1, 1) - m(2, 2);
	float ytemp = m(1, 1) - m(0, 0) - m(2, 2);
	float ztemp = m(2, 2) - m(0, 0) - m(1, 1);
	int32u biggestIndex = 0;
	float biggestValue = wtemp;
	if(xtemp > biggestValue)
	{
		biggestIndex = 1;
		biggestValue = xtemp;
	}
	if(ytemp > biggestValue)
	{
		biggestIndex = 2;
		biggestValue = ytemp;
	}
	if(ztemp > biggestValue)
	{
		biggestIndex = 3;
		biggestValue = ztemp;
	}
	biggestValue = sqrtf(biggestValue + 1) * 0.5f;
	float temp = 0.25f / biggestValue;
	switch(biggestIndex)
	{
	case 0:
		w = biggestValue;
		x = (m[M_INDEX(2, 1)] - m[M_INDEX(1, 2)]) * temp;
		y = (m[M_INDEX(0, 2)] - m[M_INDEX(2, 0)]) * temp;
		z = (m[M_INDEX(1, 0)] - m[M_INDEX(0, 1)]) * temp;
		break;
	case 1:
		x = biggestValue;
		x = (m[M_INDEX(2, 1)] - m[M_INDEX(1, 2)]) * temp;
		y = (m[M_INDEX(1, 0)] + m[M_INDEX(0, 1)]) * temp;
		z = (m[M_INDEX(2, 0)] + m[M_INDEX(0, 2)]) * temp;
		break;
	case 2:
		y = biggestValue;
		w = (m[M_INDEX(0, 2)] - m[M_INDEX(2, 0)]) * temp;
		x = (m[M_INDEX(1, 0)] + m[M_INDEX(0, 1)]) * temp;
		z = (m[M_INDEX(2, 1)] + m[M_INDEX(1, 2)]) * temp;
		break;
	case 3:
		z = biggestValue;
		w = (m[M_INDEX(1, 0)] - m[M_INDEX(0, 1)]) * temp;
		x = (m[M_INDEX(0, 2)] + m[M_INDEX(2, 0)]) * temp;
		y = (m[M_INDEX(2, 1)] + m[M_INDEX(1, 2)]) * temp;
		break;
	default:
		break;
	}
	return *this;
}

void Quaternion::normalize()
{
	float mult = Math::invSqrt(x * x + y * y + z * z + w * w);
	x *= mult;
	y *= mult;
	z *= mult;
	w *= mult;
}
